#include <Precomp.h>
#include <PositionPredictor.h>

using namespace Ogre;

void CPositionPredictor::Reset(float flBufferTime, float flNetworkInterval, float flStartTime, const Ogre::Vector3& startPosition)
{
    m_flBufferTime = flBufferTime;
    m_flNetworkInterval = flNetworkInterval;
    m_flStartTime = flStartTime;

    discretePositions.push_back(CDiscretePosition(startPosition, flStartTime + flBufferTime));

    m_flNextAddTime = m_flStartTime + m_flBufferTime + m_flNetworkInterval;
    discretePositions.clear();
}

void CPositionPredictor::AddDiscretePosition(const Vector3& vectorNew)
{
    discretePositions.push_back(CDiscretePosition(vectorNew, m_flNextAddTime));

    m_flNextAddTime += m_flNetworkInterval;
}

void CPositionPredictor::GetCurrentPosition(Vector3& vectorCurrent, float flCurrentTime)
{
    if(discretePositions.size() == 0)
    {
        vectorCurrent.x = vectorCurrent.y = vectorCurrent.z = 0.0f;
        return;
    }

    int indexBefore = -1;
    for(unsigned int i = 1; i < discretePositions.size(); i++)
    {
        if(discretePositions[i].GetTimestamp() > flCurrentTime)
        {
            indexBefore = i - 1;
            break;
        }
    }
    if(indexBefore == -1)
    {
        indexBefore = discretePositions.size() - 1;
    }

    if(indexBefore == 0 && discretePositions.size() == 1)
    {
        vectorCurrent = discretePositions[0].GetPosition();
        return;
    }

    Vector3 velocityIn(0.0f, 0.0f, 0.0f);
    Vector3 velocityOut(0.0f, 0.0f, 0.0f);

    if(indexBefore > 0)
    {
        velocityIn = (discretePositions[indexBefore + 1].GetPosition() - discretePositions[indexBefore - 1].GetPosition()) / 2.0f;
    }

    if((unsigned int)indexBefore < discretePositions.size() - 2)
    {
        velocityOut = (discretePositions[indexBefore + 2].GetPosition() - discretePositions[indexBefore].GetPosition()) / 2.0f;
    }
    else
    {
        vectorCurrent = discretePositions[indexBefore].GetPosition() + velocityIn * (1.0f * (flCurrentTime - discretePositions[indexBefore].GetTimestamp()));
        return;
    }

    float flInterval = 1.0f * (discretePositions[indexBefore + 1].GetTimestamp() - discretePositions[indexBefore].GetTimestamp());
    float flT = (flCurrentTime - discretePositions[indexBefore].GetTimestamp()) / flInterval;
    if(flT < 0.0f)
    {
        flT = 0.0f;
    }

    Vector3 vector1 = discretePositions[indexBefore].GetPosition();
    Vector3 vector4 = discretePositions[indexBefore + 1].GetPosition();

    float h00 = 2.0f * flT * flT * flT - 3.0f * flT * flT + 1.0f;
    float h01 = -2.0f * flT * flT * flT + 3.0f * flT * flT;

    float h10 = flT * flT * flT - 2.0f * flT * flT + flT;
    float h11 = flT * flT * flT - flT * flT;

    vectorCurrent = vector1 * h00 + velocityIn * h10 + velocityOut * h11 + vector4 * h01;
}

